#pragma once

#include "MapOperation.h"
#include "MapInfo.h"

#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>

#include <kybot_msg/ActiveMapSrv.h>
#include <kybot_msg/GetActivedMapNameSrv.h>
#include <kybot_msg/SetCurrentPoseSrv.h>
#include <kybot_msg/GetChargeStationPoseSrv.h>
#include <kybot_msg/SetChargeStationPoseSrv.h>
#include <MapIO.h>
#include <kybot_msg/StartLocalization.h>
#include <kybot_msg/GetLocationStatusSrv.h>
#include <kybot_msg/GetCurrentModeSrv.h>
#include <boost/shared_ptr.hpp>
#include <boost/thread.hpp>
#include <opencv2/opencv.hpp>
#include <string>
#include <kybot_msg/NodeInfoSrv.h>
#include<kybot_msg/AddLandMark.h>
#include<std_msgs/Bool.h>
#include "kybot_msg/BaseInfo.h"
#include <bosch_locator_bridge/ClientControlMode.h>
#include <bosch_locator_bridge/ClientLocalizationVisualization.h>
#include <glog/logging.h>
#include <kybot_msg/SwitchMapSrv.h>
#include <kybot_msg/SearchMapSrv.h>



namespace kybot_map {
class MapManager {
public:
    MapManager();
    ~MapManager();

    void initialize();
    void run();

    inline geometry_msgs::Pose2D GetPose() const {return pose_;}
    inline int GetLocalizationStatus() const {return status_;}
    const sensor_msgs::PointCloud2& GetLaserScan() const {return laser_scan_;}
    inline int GetMode() const {return mode_;}

private:
    bool activeMapSrvCallback(kybot_msg::ActiveMapSrv::Request &req, kybot_msg::ActiveMapSrv::Response &res);	
    bool getActivedMapNameSrvCallback(kybot_msg::GetActivedMapNameSrv::Request &req, kybot_msg::GetActivedMapNameSrv::Response &res);
    bool setCurrentPoseSrvCallback(kybot_msg::SetCurrentPoseSrv::Request &req, kybot_msg::SetCurrentPoseSrv::Response &res);
    bool getChargeStationPoseSrvCallback(kybot_msg::GetChargeStationPoseSrv::Request &req, kybot_msg::GetChargeStationPoseSrv::Response &res);
    bool setChargeStationPoseSrvCallback(kybot_msg::SetChargeStationPoseSrv::Request &req, kybot_msg::SetChargeStationPoseSrv::Response &res);
    bool NodeInfoSrvCallback(kybot_msg::NodeInfoSrv::Request &req, 
							kybot_msg::NodeInfoSrv::Response &res);
    void publishNavigationMap(MapIO::MapDataPtr map);
    void publishInitialPose(geometry_msgs::Pose2D pose);
    void BaseInfoCallback(const kybot_msg::BaseInfo::ConstPtr &msg);
    bool StartLocalization(kybot_msg::StartLocalization::Request & req, kybot_msg::StartLocalization::Response & res);
    bool GetLocationStatus(kybot_msg::GetLocationStatusSrv::Request & req, kybot_msg::GetLocationStatusSrv::Response & res);
    bool GetCurrentMode(kybot_msg::GetCurrentModeSrv::Request & req, kybot_msg::GetCurrentModeSrv::Response & res);
    
	bool SwitchMapCallback(kybot_msg::SwitchMapSrv::Request &req,kybot_msg::SwitchMapSrv::Response &res);
    bool SearchMapCallback(kybot_msg::SearchMapSrv::Request &req,kybot_msg::SearchMapSrv::Response &res);

    bool Shutdownsaveposeinfile();
    bool Bootinitpose();
    void Boot_and_location();
    bool checkChargepose();
    boost::shared_ptr<boost::thread> BootandlocationThread_;
    void InitGlog(const std::string& log_dir, const std::string& log_name);

private:
    ros::NodeHandle nh_;

    ros::Publisher locatingMapPublisher_;
    ros::Publisher planningMapPuber_;
    ros::Publisher initialPosePuber_;
    ros::Publisher relocationflagPuber_;
    ros::Subscriber baseInfoSub_;
    ros::ServiceServer activeMapSrv_;
    ros::ServiceServer getActivedMapNameSrv_;
    //坐标重定位，设置当前坐标
    ros::ServiceServer setCurrentPoseSrv_;
    ros::ServiceServer getChargeStationPoseSrv_;
    ros::ServiceServer setChargeStationPoseSrv_;
    // 开始/结束定位
    ros::ServiceServer startLocalizationSrv_;
    ros::ServiceServer get_location_statusSrv;
    ros::ServiceServer get_current_mode_Srv;
    ros::Publisher laser_puber_;

    ros::Subscriber laser_sub_;

    ros::Subscriber status_sub_;
    ros::Subscriber pose_sub_;
    ros::Subscriber mode_sub_;
	ros::ServiceServer Search_Map_Srv;
    ros::ServiceServer Switch_Map_Srv;

    ros::ServiceServer node_info_srv_;
    
    boost::shared_ptr<kybot_map::MapOperation> map_operation_;
    boost::shared_ptr<kybot_map::MapInfo> map_info_;
    std::string bootposefilename_;
    bool isCharging;
    bool chargeButton;
    //激光数据
    sensor_msgs::PointCloud2 laser_scan_;
    //定位状态
    int status_;
    //坐标
    geometry_msgs::Pose2D pose_;
    
    int mode_ = 0;
	bool usecheckchargepose_;
    std::string mappath_;

private:
    cv::Point worldPointToCVPoint(const geometry_msgs::Point32 &point, const nav_msgs::MapMetaData &mapInfo)
    {
        auto x = (point.x - mapInfo.origin.position.x) / mapInfo.resolution;
        auto y = (point.y - mapInfo.origin.position.y) / mapInfo.resolution;
        return {static_cast<int>(x), static_cast<int>(y)};
    }
    bool isWorldPointInMap(geometry_msgs::Point32 &point, const nav_msgs::MapMetaData &mapInfo)
    {
        int mapX = static_cast<int>((point.x - mapInfo.origin.position.x) / mapInfo.resolution);
        int mapY = static_cast<int>((point.y - mapInfo.origin.position.y) / mapInfo.resolution);

        bool beyondMap = (mapX<0 || mapX>mapInfo.width || mapY<0 || mapY>mapInfo.height);
        return !beyondMap;
    }
};
}